#ifndef G2O_PNP_EDGE_H_
#define G2O_PNP_EDGE_H_

#include "g2o_icp_vertex.h"

#include <iostream>
#include <g2o/core/base_unary_edge.h>

// edge used in g2o ba
//   - edge: (_point_3d, _measurement)
//   - H = J'J, g=-J'e, dξ = H.ldlt().solve(g) in GN
//   - J: _jacobianOplusXi
//   - e: _error
//   - dξ: [dρ, dφ]' as a update is injected to the vertex
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}

    // compute the projection error
    virtual void computeError() override
    {
        const VertexPose *pose = static_cast<const VertexPose *>(_vertices[0]);
        _error = _measurement - pose->estimate() * _point;
    }

    // compute Jacobian J

    //     *         1  N
    //    ξ = argmin - SUM ||pi-exp(ξ^)pi'||^2
    //          ξ    2 i=1
    //
    // e(ξ) = pi-exp(ξ^)pi'
    //
    //        de     d(Tp)    [I -(Rp+t)^]
    //    J = -- = - ------ = |          | 
    //        dξ       dξ     [0'    0'  ]
    virtual void linearizeOplus() override
    {
        VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);
        Sophus::SE3d T = pose->estimate();
        Eigen::Vector3d xyz_trans = T * _point;
        _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
        _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
    }

    bool read(istream &in) override { return false; }
    bool write(ostream &out) const override { return false; }

protected:
    Eigen::Vector3d _point;
};

#endif
